#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>

#include <sys/types.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/ip6.h>
#include <netinet/icmp6.h>
#include <netinet/ether.h>
#include <netinet/ip_icmp.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <errno.h>
#include <sys/select.h>
#include <net/if.h>
#include <time.h>

#define DAT_LEN 56
char buf[BUFSIZ] = { 0 };

uint16_t checksum(uint16_t *addr, int len)
{
	int count = len;
	register uint32_t sum = 0;
	uint16_t answer = 0;

	// Sum up 2-byte values until none or only one byte left.
	while (count > 1) {
		sum += *(addr++);
		count -= 2;
	}

	// Add left-over byte, if any.
	if (count > 0) {
		sum += *(uint8_t *)addr;
	}

	// Fold 32-bit sum into 16 bits; we lose information by doing this,
	// increasing the chances of a collision.
	// sum = (lower 16 bits) + (upper 16 bits shifted right 16 bits)
	while (sum >> 16) {
		sum = (sum & 0xffff) + (sum >> 16);
	}

	// Checksum is one's compliment of sum.
	answer = ~sum;

	return (answer);
}

int send_v6(int fd, uint16_t seq, struct addrinfo *peer, const char *ifname)
{
	struct icmp6_hdr *icmp = (void *)buf;
	icmp->icmp6_type = ICMP6_ECHO_REQUEST;
	icmp->icmp6_code = 0;
	icmp->icmp6_cksum = 0;
	icmp->icmp6_id = htons(getpid());
	icmp->icmp6_seq = htons(seq);
	memset(icmp + 1, 0x58, DAT_LEN);
	gettimeofday((void *)(icmp + 1), NULL);
	struct sockaddr_in6 *addr = (void *)peer->ai_addr;
	if (IN6_IS_ADDR_LINKLOCAL(&addr->sin6_addr)) {
		addr->sin6_scope_id = if_nametoindex(ifname);
	}
	return sendto(fd, icmp, 8 + DAT_LEN, 0, peer->ai_addr, peer->ai_addrlen);
}

int proc_v6(void *msg, int rlen)
{
	char ip[128];
	struct icmp6_hdr *icmp = msg;

	if (icmp->icmp6_id == htons(getpid()) && icmp->icmp6_type == ICMP6_ECHO_REPLY) {
		return 1;
	}
	return 0;
}

int send_v4(int fd, uint16_t seq, struct addrinfo *peer, const char *ifname)
{
	uint16_t sum;
	struct icmp *icmp = (void *)buf;
	icmp->icmp_type = ICMP_ECHO;
	icmp->icmp_code = 0;
	icmp->icmp_id = htons(getpid());
	icmp->icmp_seq = htons(seq);
	icmp->icmp_cksum = 0;
	memset(icmp->icmp_data, 0x5A, DAT_LEN);
	gettimeofday((void *)icmp->icmp_data, NULL);
	sum = checksum((void *)icmp, 8 + DAT_LEN);
	icmp->icmp_cksum = sum;
	return sendto(fd, icmp, 8 + DAT_LEN, 0, peer->ai_addr, peer->ai_addrlen);
}

int proc_v4(void *msg, int rlen)
{
	struct ip *ip = msg;
	struct icmp *icmp = (void *)((char *)ip + (ip->ip_hl << 2));

	if (icmp->icmp_id == htons(getpid()) && icmp->icmp_type == ICMP_ECHOREPLY) {
		return 1;
	}
	return 0;
}

int (*proc_icmp)(void *msg, int rlen);
int (*send_icmp)(int fd, uint16_t seq, struct addrinfo *peer, const char *ifname);

int test_icmp(const char *ip, const char *ifname, int time_out)
{
	struct addrinfo hints, *result, *rp;
	bzero(&hints, sizeof(hints));
	hints.ai_family = AF_UNSPEC;
	hints.ai_socktype = SOCK_RAW;
	hints.ai_protocol = 0;
	int ret = 0, fd = -1, len = 0;

	if ((ret = getaddrinfo(ip, NULL, &hints, &result)) < 0) {
		perror(gai_strerror(ret));
		exit(0);
	}
	struct timeval tv = { time_out + 1, 0 };
	for (rp = result; rp; rp = rp->ai_next) {
		if ((fd = socket(rp->ai_family, rp->ai_socktype, rp->ai_family == AF_INET ? IPPROTO_ICMP : IPPROTO_ICMPV6)) < 0) {
			perror("socket");
			continue;
		}
		if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv))) {
			perror("setsockopt");
			close(fd);
			continue;
		}

		char host[INET6_ADDRSTRLEN];
		getnameinfo(rp->ai_addr, rp->ai_addrlen, host, sizeof(host), NULL, 0, NI_NUMERICHOST);
		fprintf(stderr, "ping test:%s", host);

		if (rp->ai_family == AF_INET) {
			send_icmp = send_v4;
			proc_icmp = proc_v4;
		} else {
			send_icmp = send_v6;
			proc_icmp = proc_v6;
		}
		send_icmp(fd, 1, rp, ifname);
		send_icmp(fd, 2, rp, ifname);

		time_t tm = time(NULL);
		while (time(NULL) - tm < time_out) {
			if ((len = recv(fd, buf, sizeof(buf), 0)) < 0) {
				continue;
			}
			if (proc_icmp((void *)buf, len)) {
				freeaddrinfo(result);
				close(fd);
				printf(" OK\n");
				return 1;
			}
		}
		close(fd);
		printf(" FAIL\n");
		continue;
	}
	freeaddrinfo(result);
	return 0;
}

// int main(int argc, char **argv)
// {
// 	if (test_icmp(argv[1], 2)) {
// 		printf("OK\n");
// 	} else {
// 		printf("fail\n");
// 	}
// 	return 0;
// }